
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       param.c
  * @author     baiyang
  * @date       2021-7-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdio.h>
#include <float.h>
#include <stdbool.h>

#include <dfs_posix.h>
#include <string.h>

#include "param.h"
#include <file_manager/file_manager.h>

#include <mavproxy/mavlink_param.h>

#include <common/yxml/yxml.h>
#include <common/gp_math/gp_mathlib.h>
#include <board_config/borad_config.h>
#include <notify/notify.h>
/*-----------------------------------macro------------------------------------*/
#define YXML_STACK_SIZE                1024

#define PARAM_FILE_NAME                "/sys/param.xml"
#define PARAM_DIR                      "/sys"

#define PARAM_GROUP_COUNT (sizeof(param_list_t) / sizeof(param_group_t))
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
/* 步骤四:   在组中定义参数 */
PARAM_GROUP(INS)
PARAM_DECLARE_GROUP(INS) = \
{ \
    PARAM_DEFINE_FLOAT(INS_GYROFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYROFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYROFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_Z, 0.0f),
    PARAM_DEFINE_INT16(INS_GYRO_FILTER, 20),
    PARAM_DEFINE_INT16(INS_ACCEL_FILTER, 20),
    PARAM_DEFINE_INT8(INS_USE, 1),
    PARAM_DEFINE_INT8(INS_USE2, 1),
    PARAM_DEFINE_INT8(INS_USE3, 1),
    PARAM_DEFINE_FLOAT(INS_STILL_THRESH, 2.5f),
    PARAM_DEFINE_INT8(INS_GYR_CAL, 1),
    PARAM_DEFINE_INT8(INS_TRIM_OPTION, 1),
    PARAM_DEFINE_INT8(INS_ACC_BODYFIX, 2),
    PARAM_DEFINE_FLOAT(INS_POS1_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS1_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS1_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_Z, 0.0f),
    PARAM_DEFINE_INT32(INS_GYR_ID, 0),
    PARAM_DEFINE_INT32(INS_GYR2_ID, 0),
    PARAM_DEFINE_INT32(INS_GYR3_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC2_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC3_ID, 0),
    PARAM_DEFINE_INT8(INS_FAST_SAMPLE, 1),
    PARAM_DEFINE_INT8(INS_ENABLE_MASK, 0x7F),
    PARAM_DEFINE_INT8(INS_GYRO_RATE, 0),
};

PARAM_GROUP(BARO)
PARAM_DECLARE_GROUP(BARO) = \
{ \
    PARAM_DEFINE_FLOAT(BARO1_GND_PRESS, 0.0f),
    PARAM_DEFINE_FLOAT(BARO_GND_TEMP,   0.0f),
    PARAM_DEFINE_FLOAT(BARO_ALT_OFFSET, 0.0f),
    PARAM_DEFINE_INT8(BARO_PRIMARY,     0),
    PARAM_DEFINE_INT8(BARO_EXT_BUS,     0),
    PARAM_DEFINE_FLOAT(BARO_SPEC_GRAV,  1.0f),
    PARAM_DEFINE_FLOAT(BARO2_GND_PRESS, 0.0f),
    PARAM_DEFINE_FLOAT(BARO3_GND_PRESS, 0.0f),
    PARAM_DEFINE_INT8(BARO_FLTR_RNG,    0),
    PARAM_DEFINE_INT32(BARO_PROBE_EXT,  0),
    PARAM_DEFINE_INT32(BARO1_DEVID,     0),
    PARAM_DEFINE_INT32(BARO2_DEVID,     0),
    PARAM_DEFINE_INT32(BARO3_DEVID,     0),
};

PARAM_GROUP(COMPASS)
PARAM_DECLARE_GROUP(COMPASS) = \
{ \
    PARAM_DEFINE_INT8(COMPASS_USE, 1),
    PARAM_DEFINE_FLOAT(COMPASS_OFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DEC,   0.0f),
    PARAM_DEFINE_INT8(COMPASS_LEARN,  0),
    PARAM_DEFINE_INT8(COMPASS_AUTODEC, 1),
    PARAM_DEFINE_INT8(COMPASS_MOTCT, 0),
    PARAM_DEFINE_FLOAT(COMPASS_MOT_X, 0),
    PARAM_DEFINE_FLOAT(COMPASS_MOT_Y, 0),
    PARAM_DEFINE_FLOAT(COMPASS_MOT_Z, 0),
    PARAM_DEFINE_INT8(COMPASS_ORIENT, 0),
    PARAM_DEFINE_INT8(COMPASS_EXTERNAL, 0),

    PARAM_DEFINE_INT8(COMPASS_USE2, 1),
    PARAM_DEFINE_FLOAT(COMPASS_OFS2_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS2_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT2_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT2_Z, 0.0f),
    PARAM_DEFINE_INT8(COMPASS_ORIENT2, 0),
    PARAM_DEFINE_INT8(COMPASS_EXTERN2, 0),

    PARAM_DEFINE_INT8(COMPASS_USE3, 1),
    PARAM_DEFINE_FLOAT(COMPASS_OFS3_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_OFS3_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT3_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_MOT3_Z, 0.0f),
    PARAM_DEFINE_INT8(COMPASS_ORIENT3, 0),
    PARAM_DEFINE_INT8(COMPASS_EXTERN3, 0),

    PARAM_DEFINE_INT32(COMPASS_DEV_ID,  0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID2, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID3, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID4, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID5, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID6, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID7, 0),
    PARAM_DEFINE_INT32(COMPASS_DEV_ID8, 0),

    PARAM_DEFINE_FLOAT(COMPASS_DIA_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI_Z, 0.0f),

    PARAM_DEFINE_FLOAT(COMPASS_DIA2_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA2_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI2_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI2_Z, 0.0f),

    PARAM_DEFINE_FLOAT(COMPASS_DIA3_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_DIA3_Z, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI3_X, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_ODI3_Z, 0.0f),
    
    PARAM_DEFINE_FLOAT(COMPASS_CAL_FIT, 16.0f),
    PARAM_DEFINE_INT32(COMPASS_TYPEMASK, 0),
    PARAM_DEFINE_INT16(COMPASS_OFFS_MAX,1800),
    PARAM_DEFINE_INT8(COMPASS_FLTR_RNG, 0),
    PARAM_DEFINE_INT8(COMPASS_AUTO_ROT, 2),
    PARAM_DEFINE_INT32(COMPASS_PRIO1_ID, 0),
    PARAM_DEFINE_INT32(COMPASS_PRIO2_ID, 0),
    PARAM_DEFINE_INT32(COMPASS_PRIO3_ID, 0),
    
    PARAM_DEFINE_INT8(COMPASS_ENABLE, 1),

    PARAM_DEFINE_FLOAT(COMPASS_SCALE,  0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_SCALE2, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_SCALE3, 0.0f),

    PARAM_DEFINE_INT16(COMPASS_OPTIONS, 0),

    PARAM_DEFINE_FLOAT(COMPASS_CUS_ROLL, 0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_CUS_PIT,  0.0f),
    PARAM_DEFINE_FLOAT(COMPASS_CUS_YAW,  0.0f),
};

//位置控制参数
PARAM_GROUP(POS_CTRL) 
PARAM_DECLARE_GROUP(POS_CTRL) = \
{
    PARAM_DEFINE_FLOAT(PSC_POSZ_P, 1.0f),
    
    PARAM_DEFINE_FLOAT(PSC_VELZ_P, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_I, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_D, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_IMAX, 1000.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FLTE, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELZ_FLTD, 5.0f),
    
    PARAM_DEFINE_FLOAT(PSC_ACCZ_P, 0.5f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_I, 1.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_D, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_IMAX, 800.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTT, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTE, 20.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_FLTD, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_ACCZ_SMAX, 0.0f),
    
    PARAM_DEFINE_FLOAT(PSC_POSXY_P, 1.0f),
    
    PARAM_DEFINE_FLOAT(PSC_VELXY_P, 2.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_I, 1.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_D, 0.5f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_IMAX, 1000.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FF, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FLTE, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_VELXY_FLTD, 5.0f),

    PARAM_DEFINE_FLOAT(PSC_ANGLE_MAX, 0.0f),
    PARAM_DEFINE_FLOAT(PSC_JERK_XY, 5.0f),
    PARAM_DEFINE_FLOAT(PSC_JERK_Z, 5.0f),
};

PARAM_GROUP(ATT_CTRL) 
PARAM_DECLARE_GROUP(ATT_CTRL) = \
{ \
    PARAM_DEFINE_FLOAT(ATC_SLEW_YAW,6000.0f),
    PARAM_DEFINE_FLOAT(ATC_ANG_RLL_P,4.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_P,0.135f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_I,0.135f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_D,0.0036f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_IMAX,0.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_FLTT,20.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_FLTE,0.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_RLL_FLTD,20.0f),
    PARAM_DEFINE_FLOAT(ATC_ANG_PIT_P,4.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_P,0.135f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_I,0.135f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_D,0.0036f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_IMAX,0.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_FLTT,20.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_FLTE,0.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_PIT_FLTD,20.0f),
    PARAM_DEFINE_FLOAT(ATC_ANG_YAW_P,4.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_P,0.180f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_I,0.018f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_D,0.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_IMAX,0.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_FLTT,20.0f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_FLTE,2.5f),
    PARAM_DEFINE_FLOAT(ATC_RAT_YAW_FLTD,0.0f),
    PARAM_DEFINE_FLOAT(ATC_THR_MIX_MIN,0.1f),
    PARAM_DEFINE_FLOAT(ATC_THR_MIX_MAN,0.5f),
    PARAM_DEFINE_FLOAT(ATC_THR_MIX_MAX,0.5f),
    PARAM_DEFINE_FLOAT(ATC_ACCEL_R_MAX,110000.0f),
    PARAM_DEFINE_FLOAT(ATC_ACCEL_P_MAX,110000.0f),
    PARAM_DEFINE_FLOAT(ATC_ACCEL_Y_MAX,27000.0f),
    PARAM_DEFINE_FLOAT(ATC_RATE_R_MAX,0.0f),
    PARAM_DEFINE_FLOAT(ATC_RATE_P_MAX,0.0f),
    PARAM_DEFINE_FLOAT(ATC_RATE_Y_MAX,0.0f),
    PARAM_DEFINE_INT32(ATC_RATE_FF_EN,1),
    PARAM_DEFINE_INT32(ATC_ANGLE_BOOST,1),
    PARAM_DEFINE_FLOAT(ATC_INPUT_TC,0.2f),
    PARAM_DEFINE_FLOAT(ATC_ANG_LIM_TC,1.0f),
};

PARAM_GROUP(VEHICLE)
PARAM_DECLARE_GROUP(VEHICLE) = {
    /* Determines when to start and stop logging (Blog).
    0: when armed, start logging,when disarm, stop
    1: when armed, start logging,when Power-off,stop
    2: when Power-on, start logging*/
    PARAM_DEFINE_INT32(BLOG_MODE, 0),
    PARAM_DEFINE_INT16(ANGLE_MAX,4500),
    // @Description: Controls major frame class for multicopter component
    // @Values: 0:Undefined, 1:Quad, 2:Hexa, 3:Octa, 4:OctaQuad, 5:Y6, 6:Heli, 7:Tri, 8:SingleCopter, 9:CoaxCopter, 10:BiCopter, 11:Heli_Dual, 12:DodecaHexa, 13:HeliQuad, 14:Deca
    PARAM_DEFINE_INT8(FRAME_CLASS,0),
    // @Description: Controls motor mixing for multicopters.  Not used for Tri or Traditional Helicopters.
    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed
    PARAM_DEFINE_INT8(FRAME_TYPE,1),
    PARAM_DEFINE_INT8(FS_THR_ENABLE, 1),
    PARAM_DEFINE_INT16(FS_THR_VALUE, 975),
    PARAM_DEFINE_INT16(THR_DZ, 100),

    // 记录IMU到日志中的速度, 0:25Hz, 1:100Hz, 2:200Hz, 3: 500, 4:全速
    PARAM_DEFINE_UINT16(BLOG_IMU_SPEED, 0),
    PARAM_DEFINE_FLOAT(PILOT_THR_FILT,0.0f),
    PARAM_DEFINE_FLOAT(PILOT_Y_RATE,PILOT_Y_RATE_DEFAULT),
    PARAM_DEFINE_FLOAT(ACRO_Y_EXPO,ACRO_Y_EXPO_DEFAULT),

    // @Param: PILOT_THR_BHV
    // @DisplayName: Throttle stick behavior
    // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick.
    // @User: Standard
    // @Values: 0:None,1:Feedback from mid stick,2:High throttle cancels landing,4:Disarm on land detection
    // @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection
    PARAM_DEFINE_INT16(PILOT_THR_BHV,0),
    PARAM_DEFINE_INT8(DISARM_DELAY,10),

    // 单位：cm/s
    PARAM_DEFINE_INT16(PILOT_SPEED_UP,250),
    PARAM_DEFINE_INT16(PILOT_SPEED_DN,0),
    PARAM_DEFINE_INT16(PILOT_ACCEL_Z,250),
    PARAM_DEFINE_FLOAT(PILOT_TKOFF_ALT, 0.0f),
    PARAM_DEFINE_INT8(AHRS_ORIENTATION, 0),

    PARAM_DEFINE_INT8(ESC_CALIBRATION, 0),
    PARAM_DEFINE_INT16(GPS_HDOP_GOOD, 140),
};

PARAM_GROUP(WPNAV)
PARAM_DECLARE_GROUP(WPNAV) = {
    PARAM_DEFINE_FLOAT(WPNAV_SPEED, 500.0f),
    PARAM_DEFINE_FLOAT(WPNAV_RADIUS, 200.0f),
    PARAM_DEFINE_FLOAT(WPNAV_SPEED_UP, 250.0f),
    PARAM_DEFINE_FLOAT(WPNAV_SPEED_D, 150.0f),
    PARAM_DEFINE_FLOAT(WPNAV_ACC_XY, 100.0f),
    PARAM_DEFINE_FLOAT(WPNAV_ACC_Z, 100.0f),
    PARAM_DEFINE_FLOAT(WPNAV_RFND_USE, 1.0f),
    PARAM_DEFINE_FLOAT(WPNAV_ALT_MIN, 2.0f),
    PARAM_DEFINE_INT16(WPNAV_LAND_SPEED, 200),
    PARAM_DEFINE_INT16(WPNAV_LAND_ALT, 300),  
};

PARAM_GROUP(LOITER)
PARAM_DECLARE_GROUP(LOITER) = {
    PARAM_DEFINE_FLOAT(LOITER_ANG_MAX, 0.0f),
    PARAM_DEFINE_FLOAT(LOITER_SPEED, 1250.0f),
    PARAM_DEFINE_FLOAT(LOITER_ACC_MAX, 500.0f),
    PARAM_DEFINE_FLOAT(LOITER_BK_ACC, 250.0f),
    PARAM_DEFINE_FLOAT(LOITER_BK_JERK, 500.0f),
    PARAM_DEFINE_FLOAT(LOITER_BK_DELAY, 1.0f),
};

PARAM_GROUP(RC)
PARAM_DECLARE_GROUP(RC) = {
    PARAM_DEFINE_INT8(RCMAP_ROLL, 1),
    PARAM_DEFINE_INT8(RCMAP_PITCH, 2),
    PARAM_DEFINE_INT8(RCMAP_YAW, 4),
    PARAM_DEFINE_INT8(RCMAP_THROTTLE, 3),
    PARAM_DEFINE_INT16(RC1_MIN, 1100),
    PARAM_DEFINE_INT16(RC1_MAX, 1900),
    PARAM_DEFINE_INT16(RC1_TRIM, 1500),
    PARAM_DEFINE_INT16(RC1_DZ, 20),
    PARAM_DEFINE_INT16(RC1_OPTION, 0),
    PARAM_DEFINE_INT16(RC2_MIN, 1100),
    PARAM_DEFINE_INT16(RC2_MAX, 1900),
    PARAM_DEFINE_INT16(RC2_TRIM, 1500),
    PARAM_DEFINE_INT16(RC2_DZ, 20),
    PARAM_DEFINE_INT16(RC2_OPTION, 0),
    PARAM_DEFINE_INT16(RC3_MIN, 1100),
    PARAM_DEFINE_INT16(RC3_MAX, 1900),
    PARAM_DEFINE_INT16(RC3_TRIM, 1500),
    PARAM_DEFINE_INT16(RC3_DZ, 30),
    PARAM_DEFINE_INT16(RC3_OPTION, 0),
    PARAM_DEFINE_INT16(RC4_MIN, 1100),
    PARAM_DEFINE_INT16(RC4_MAX, 1900),
    PARAM_DEFINE_INT16(RC4_TRIM, 1500),
    PARAM_DEFINE_INT16(RC4_DZ, 20),
    PARAM_DEFINE_INT16(RC4_OPTION, 0),
    PARAM_DEFINE_INT16(RC5_MIN, 1100),
    PARAM_DEFINE_INT16(RC5_MAX, 1900),
    PARAM_DEFINE_INT16(RC5_TRIM, 1500),
    PARAM_DEFINE_INT16(RC5_DZ, 20),
    PARAM_DEFINE_INT16(RC5_OPTION, 0),
    PARAM_DEFINE_INT16(RC6_MIN, 1100),
    PARAM_DEFINE_INT16(RC6_MAX, 1900),
    PARAM_DEFINE_INT16(RC6_TRIM, 1500),
    PARAM_DEFINE_INT16(RC6_DZ, 0),
    PARAM_DEFINE_INT16(RC6_OPTION, 0),
    PARAM_DEFINE_INT16(RC7_MIN, 1100),
    PARAM_DEFINE_INT16(RC7_MAX, 1900),
    PARAM_DEFINE_INT16(RC7_TRIM, 1500),
    PARAM_DEFINE_INT16(RC7_DZ, 0),
    PARAM_DEFINE_INT16(RC7_OPTION, 0),
    PARAM_DEFINE_INT16(RC8_MIN, 1100),
    PARAM_DEFINE_INT16(RC8_MAX, 1900),
    PARAM_DEFINE_INT16(RC8_TRIM, 1500),
    PARAM_DEFINE_INT16(RC8_DZ, 0),
    PARAM_DEFINE_INT16(RC8_OPTION, 0),
    PARAM_DEFINE_INT16(RC9_MIN, 1100),
    PARAM_DEFINE_INT16(RC9_MAX, 1900),
    PARAM_DEFINE_INT16(RC9_TRIM, 1500),
    PARAM_DEFINE_INT16(RC9_DZ, 0),
    PARAM_DEFINE_INT16(RC9_OPTION, 0),
    PARAM_DEFINE_INT16(RC10_MIN, 1100),
    PARAM_DEFINE_INT16(RC10_MAX, 1900),
    PARAM_DEFINE_INT16(RC10_TRIM, 1500),
    PARAM_DEFINE_INT16(RC10_DZ, 0),
    PARAM_DEFINE_INT16(RC10_OPTION, 0),
    PARAM_DEFINE_INT16(RC11_MIN, 1100),
    PARAM_DEFINE_INT16(RC11_MAX, 1900),
    PARAM_DEFINE_INT16(RC11_TRIM, 1500),
    PARAM_DEFINE_INT16(RC11_DZ, 20),
    PARAM_DEFINE_INT16(RC11_OPTION, 0),
    PARAM_DEFINE_INT16(RC12_MIN, 1100),
    PARAM_DEFINE_INT16(RC12_MAX, 1900),
    PARAM_DEFINE_INT16(RC12_TRIM, 1500),
    PARAM_DEFINE_INT16(RC12_DZ, 20),
    PARAM_DEFINE_INT16(RC12_OPTION, 0),
    PARAM_DEFINE_INT16(RC13_MIN, 1100),
    PARAM_DEFINE_INT16(RC13_MAX, 1900),
    PARAM_DEFINE_INT16(RC13_TRIM, 1500),
    PARAM_DEFINE_INT16(RC13_DZ, 30),
    PARAM_DEFINE_INT16(RC13_OPTION, 0),
    PARAM_DEFINE_INT16(RC14_MIN, 1100),
    PARAM_DEFINE_INT16(RC14_MAX, 1900),
    PARAM_DEFINE_INT16(RC14_TRIM, 1500),
    PARAM_DEFINE_INT16(RC14_DZ, 20),
    PARAM_DEFINE_INT16(RC14_OPTION, 0),
    PARAM_DEFINE_INT16(RC15_MIN, 1100),
    PARAM_DEFINE_INT16(RC15_MAX, 1900),
    PARAM_DEFINE_INT16(RC15_TRIM, 1500),
    PARAM_DEFINE_INT16(RC15_DZ, 20),
    PARAM_DEFINE_INT16(RC15_OPTION, 0),
    PARAM_DEFINE_INT16(RC16_MIN, 1100),
    PARAM_DEFINE_INT16(RC16_MAX, 1900),
    PARAM_DEFINE_INT16(RC16_TRIM, 1500),
    PARAM_DEFINE_INT16(RC16_DZ, 0),
    PARAM_DEFINE_INT16(RC16_OPTION, 0),
    PARAM_DEFINE_INT16(RC17_MIN, 1100),
    PARAM_DEFINE_INT16(RC17_MAX, 1900),
    PARAM_DEFINE_INT16(RC17_TRIM, 1500),
    PARAM_DEFINE_INT16(RC17_DZ, 0),
    PARAM_DEFINE_INT16(RC17_OPTION, 0),
    PARAM_DEFINE_INT16(RC18_MIN, 1100),
    PARAM_DEFINE_INT16(RC18_MAX, 1900),
    PARAM_DEFINE_INT16(RC18_TRIM, 1500),
    PARAM_DEFINE_INT16(RC18_DZ, 0),
    PARAM_DEFINE_INT16(RC18_OPTION, 0),
};

// 0:ANGLE_MODE, 1: ANGLE_RATE_MODE, 2: ALT_HOLD_MODE, 3: AUTO_MODE, 5:LOITER_MODE
PARAM_GROUP(FLIGHTMODE)
PARAM_DECLARE_GROUP(FLIGHTMODE) = {
    PARAM_DEFINE_UINT8(FLTMODE_CH,5),
    PARAM_DEFINE_UINT8(FLTMODE1,0),
    PARAM_DEFINE_UINT8(FLTMODE2,0),
    PARAM_DEFINE_UINT8(FLTMODE3,0),
    PARAM_DEFINE_UINT8(FLTMODE4,0),
    PARAM_DEFINE_UINT8(FLTMODE5,0),
    PARAM_DEFINE_UINT8(FLTMODE6,0),
};

PARAM_GROUP(SERIAL)
PARAM_DECLARE_GROUP(SERIAL) = {
    PARAM_DEFINE_INT8(SERIAL1_PROTOCOL,2),
    PARAM_DEFINE_INT32(SERIAL1_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL2_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL2_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL3_PROTOCOL,5),
    PARAM_DEFINE_INT32(SERIAL3_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL4_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL4_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL5_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL5_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL6_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL6_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL7_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL7_BAUD,57),
    PARAM_DEFINE_INT8(SERIAL8_PROTOCOL,-1),
    PARAM_DEFINE_INT32(SERIAL8_BAUD,57),
};

PARAM_GROUP(MOTOR)
PARAM_DECLARE_GROUP(MOTOR) = {
    PARAM_DEFINE_INT16(MOT_YAW_HEADROOM,200),
    PARAM_DEFINE_FLOAT(MOT_THST_EXPO,0.65f),
    PARAM_DEFINE_FLOAT(MOT_SPIN_MAX,0.95f),
    PARAM_DEFINE_FLOAT(MOT_BAT_VOLT_MAX,0.0f),
    PARAM_DEFINE_FLOAT(MOT_BAT_VOLT_MIN,0.0f),
    PARAM_DEFINE_FLOAT(MOT_BAT_CURR_MAX,0.0f),
    PARAM_DEFINE_INT8(MOT_PWM_TYPE,0),
    PARAM_DEFINE_INT16(MOT_PWM_MIN,0),
    PARAM_DEFINE_INT16(MOT_PWM_MAX,0),
    PARAM_DEFINE_FLOAT(MOT_SPIN_MIN,0.15f),
    PARAM_DEFINE_FLOAT(MOT_SPIN_ARM,0.10f),
    PARAM_DEFINE_FLOAT(MOT_BAT_CURR_TC,5.0f),
    PARAM_DEFINE_FLOAT(MOT_THST_HOVER,0.35f),
    PARAM_DEFINE_INT8(MOT_HOVER_LEARN,2),
    PARAM_DEFINE_INT8(MOT_SAFE_DISARM,0),
    PARAM_DEFINE_FLOAT(MOT_YAW_SV_ANGLE,30.0f),
    PARAM_DEFINE_FLOAT(MOT_SPOOL_TIME,0.5f),
    PARAM_DEFINE_FLOAT(MOT_BOOST_SCALE,0.0f),
    PARAM_DEFINE_INT8(MOT_BAT_IDX,0),
    PARAM_DEFINE_FLOAT(MOT_SLEW_UP_TIME,0.0f),
    PARAM_DEFINE_FLOAT(MOT_SLEW_DN_TIME,0.0f),
    PARAM_DEFINE_FLOAT(MOT_SAFE_TIME,1.2f),
};

PARAM_GROUP(ARMING)
PARAM_DECLARE_GROUP(ARMING) = { 
    PARAM_DEFINE_INT8(ARMING_REQUIRE, 1),
    PARAM_DEFINE_FLOAT(ARMING_ACCTHRESH, 0.75f),
    PARAM_DEFINE_INT8(ARMING_RUDDER, 2),
    PARAM_DEFINE_INT32(ARMING_MIS_ITEMS, 0),
    PARAM_DEFINE_INT32(ARMING_CHECK, 1),
};

PARAM_GROUP(NTF)
PARAM_DECLARE_GROUP(NTF) = { 
    PARAM_DEFINE_INT8(NTF_LED_BRIGHT, NOTIFY_LED_BRIGHT_DEFAULT),
    PARAM_DEFINE_INT8(NTF_BUZZ_TYPES, BUILD_DEFAULT_BUZZER_TYPE),
    PARAM_DEFINE_INT8(NTF_LED_OVERRIDE, NOTIFY_LED_OVERRIDE_DEFAULT),
    PARAM_DEFINE_INT8(NTF_BUZZ_PIN, HAL_BUZZER_PIN),
    PARAM_DEFINE_INT32(NTF_LED_TYPES, BUILD_DEFAULT_LED_TYPE),
    PARAM_DEFINE_INT8(NTF_BUZZ_ON_LVL, HAL_BUZZER_ON),
    PARAM_DEFINE_INT8(NTF_BUZZ_VOLUME, 100),
    PARAM_DEFINE_INT8(NTF_LED_LEN, NOTIFY_LED_LEN_DEFAULT),
};

PARAM_GROUP(GPS)
PARAM_DECLARE_GROUP(GPS) = { 
    PARAM_DEFINE_INT8(GPS_TYPE, 1),
    PARAM_DEFINE_INT8(GPS_TYPE2, 0),
    PARAM_DEFINE_INT8(GPS_NAVFILTER, 8),
    PARAM_DEFINE_INT8(GPS_AUTO_SWITCH, 1),
    PARAM_DEFINE_INT8(GPS_MIN_DGPS, 100),
    PARAM_DEFINE_INT8(GPS_SBAS_MODE, 2),
    PARAM_DEFINE_INT8(GPS_MIN_ELEV, -100),
    PARAM_DEFINE_INT8(GPS_INJECT_TO, 127),
    PARAM_DEFINE_INT16(GPS_SBP_LOGMASK, -256),
    PARAM_DEFINE_INT8(GPS_RAW_DATA, 0),
    PARAM_DEFINE_INT8(GPS_GNSS_MODE, 0),
    PARAM_DEFINE_INT8(GPS_SAVE_CFG, 2),
    PARAM_DEFINE_INT8(GPS_GNSS_MODE2, 0),
    PARAM_DEFINE_INT8(GPS_AUTO_CONFIG, 1),
    PARAM_DEFINE_INT16(GPS_RATE_MS, 200),
    PARAM_DEFINE_INT16(GPS_RATE_MS2, 200),
    PARAM_DEFINE_FLOAT(GPS_POS1_X, 0.0f),
    PARAM_DEFINE_FLOAT(GPS_POS1_Y, 0.0f),
    PARAM_DEFINE_FLOAT(GPS_POS1_Z, 0.0f),
    PARAM_DEFINE_FLOAT(GPS_POS2_X, 0.0f),
    PARAM_DEFINE_FLOAT(GPS_POS2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(GPS_POS2_Z, 0.0f),
    PARAM_DEFINE_INT16(GPS_DELAY_MS, 0),
    PARAM_DEFINE_INT16(GPS_DELAY_MS2, 0),
    PARAM_DEFINE_INT8(GPS_BLEND_MASK, 5),
    PARAM_DEFINE_FLOAT(GPS_BLEND_TC, 10.0f),
    PARAM_DEFINE_INT16(GPS_DRV_OPTIONS, 0),
    PARAM_DEFINE_INT8(GPS_COM_PORT, 1),
    PARAM_DEFINE_INT8(GPS_COM_PORT2, 1),
    PARAM_DEFINE_INT8(GPS_PRIMARY, 0)
};

PARAM_GROUP(LOIT)
PARAM_DECLARE_GROUP(LOIT) = { 
    PARAM_DEFINE_FLOAT(LOIT_ANG_MAX,   0.0f),
    PARAM_DEFINE_FLOAT(LOIT_SPEED,     1250.0f),
    PARAM_DEFINE_FLOAT(LOIT_ACC_MAX,   500.0f),
    PARAM_DEFINE_FLOAT(LOIT_BRK_ACCEL, 250.0f),
    PARAM_DEFINE_FLOAT(LOIT_BRK_JERK,  500.0f),
    PARAM_DEFINE_FLOAT(LOIT_BRK_DELAY, 1.0f)
};

/* 步骤三: 定义组 */
param_list_t param_list = { \
    PARAM_DEFINE_GROUP(INS),
    PARAM_DEFINE_GROUP(ATT_CTRL),
    PARAM_DEFINE_GROUP(POS_CTRL),
    PARAM_DEFINE_GROUP(VEHICLE),
    PARAM_DEFINE_GROUP(WPNAV),
    PARAM_DEFINE_GROUP(LOITER),
    PARAM_DEFINE_GROUP(RC),
    PARAM_DEFINE_GROUP(FLIGHTMODE),
    PARAM_DEFINE_GROUP(SERIAL),
    PARAM_DEFINE_GROUP(MOTOR),
    PARAM_DEFINE_GROUP(ARMING),
    PARAM_DEFINE_GROUP(BARO),
    PARAM_DEFINE_GROUP(COMPASS),
    PARAM_DEFINE_GROUP(NTF),
    PARAM_DEFINE_GROUP(GPS),
    PARAM_DEFINE_GROUP(LOIT),
};
/* 定义组结束 */

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
int _parse_xml(yxml_t *x, yxml_ret_t r, PARAM_PARSE_STATE* status)
{
    static char attr_cnt = 0;
    static char group_name[30];
    static char param_name[30];
    static char content[20];
    
    switch(*status)
    {
        case PARAM_PARSE_START:
        {
            if(r == YXML_ELEMSTART){
                if( strcmp("param_list", x->elem)==0 ){
                    *status = PARAM_PARSE_LIST;
                }
            }
        }break;
        case PARAM_PARSE_LIST:
        {
            if(r == YXML_ELEMSTART){
                if( strcmp("group", x->elem)==0 ){
                    *status = PARAM_PARSE_GROUP_INFO;
                }
            }
            if(r == YXML_ELEMEND){
                *status = PARAM_PARSE_START;
                return 0;
            }
        }break;
        case PARAM_PARSE_GROUP_INFO:
        {
            if(r == YXML_ATTRSTART){
                if( strcmp("name", x->attr)==0 ){
                    *status = PARAM_PARSE_GROUP_NAME;
                    attr_cnt = 0;
                }else{
                    //TODO
                    return 2;
                }
            }
        }break;
        case PARAM_PARSE_GROUP_NAME:
        {
            if(r == YXML_ATTRVAL){
                group_name[attr_cnt++] = x->data[0];
            }
            if(r == YXML_ATTREND){
                group_name[attr_cnt] = '\0';
                attr_cnt = 0;
                *status = PARAM_PARSE_GROUP;
            }
        }break;
        case PARAM_PARSE_GROUP:
        {
            if(r == YXML_ELEMSTART){
                if( strcmp("param", x->elem)==0 ){
                    *status = PARAM_PARSE_PARAM;
                }else{
                    //TODO
                    return 2;
                }
            }
            if(r == YXML_ELEMEND){
                *status = PARAM_PARSE_LIST;
            }
        }break;
        case PARAM_PARSE_PARAM:
        {
            if(r == YXML_ATTRSTART){
                if( strcmp("name", x->attr)==0 ){
                    *status = PARAM_PARSE_PARAM_NAME;
                    attr_cnt = 0;
                }else{
                    //TODO
                    return 2;
                }
            }
            if(r == YXML_ELEMEND){
                *status = PARAM_PARSE_GROUP;
            }
        }break;
        case PARAM_PARSE_PARAM_NAME:
        {
            if(r == YXML_ATTRVAL){
                param_name[attr_cnt++] = x->data[0];
            }
            if(r == YXML_ATTREND){
                param_name[attr_cnt] = '\0';
                attr_cnt = 0;
                *status = PARAM_PARSE_PARAM_VAL;
            }
        }break;
        case PARAM_PARSE_PARAM_VAL:
        {
            if(r == YXML_ELEMSTART){
                if( strcmp("value", x->elem)==0 ){
                    *status = PARAM_PARSE_PARAM_VAL_CONTENT;
                    attr_cnt = 0;
                }else{
                    //TODO
                    return 2;
                }
            }
        }break;
        case PARAM_PARSE_PARAM_VAL_CONTENT:
        {
            if(r == YXML_CONTENT){
                content[attr_cnt++] = x->data[0];
            }
            if(r == YXML_ELEMEND){
                content[attr_cnt] = '\0';
                attr_cnt = 0;
                *status = PARAM_PARSE_PARAM;
                
                param_set_val_by_full_name(group_name, param_name, content);
            }
        }break;
    }
    
    return 1;
}

void param_traverse(void (*param_ops)(param_t* param))
{
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;
    if (!param_ops)
        return;
    for(int j = 0 ; j < sizeof(param_list)/sizeof(param_group_t) ; j++) {
        p = gp->content;
        for(int i = 0 ; i < gp->param_num ; i++) {
                param_ops(p);
            p++;
        }
        gp++;
    }
}

param_t* param_get_by_name(const char* param_name)
{
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;
    for(int j = 0 ; j < sizeof(param_list)/sizeof(param_group_t) ; j++) {
        p = gp->content;
        for(int i= 0 ; i < gp->param_num ; i++) {
            if(strcmp(param_name, p->name) == 0)
                return p;
            p++;
        }
        gp++;
    }
    
    return NULL;
}

param_t* param_get_by_index(uint32_t index)
{
    uint32_t idx = 0;
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;
    for(int j = 0 ; j < sizeof(param_list)/sizeof(param_group_t) ; j++) {
        p = gp->content;
        for(int i= 0 ; i < gp->param_num ; i++) {
            if (idx == index) {
                return p;
            }
            p++;
            idx++;
        }
        gp++;
    }
    
    return NULL;
}

param_t* param_get_by_full_name(char* group_name, char* param_name)
{
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;
    for(int j = 0 ; j < sizeof(param_list)/sizeof(param_group_t) ; j++){
        if( strcmp(group_name, gp->name)==0 ){
            p = gp->content;
            for(int i= 0 ; i < gp->param_num ; i++){
                if( strcmp(param_name, p->name)==0 )
                    return p;
                
                p++;
            }
        }
        gp++;
    }
    
    return NULL;
}

uint32_t param_get_count(void)
{
    uint32_t count = 0;
    param_group_t* gp = (param_group_t*)&param_list;

    for (int j = 0; j < PARAM_GROUP_COUNT; j++) {
        count += gp->param_num;
        gp++;
    }

    return count;
}

int param_get_index(const param_t* param)
{
    int index = 0;
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;

    for (int i = 0; i < PARAM_GROUP_COUNT; i++) {
        p = gp->content;

        for (int j = 0; j < gp->param_num; j++) {
            if (strcmp(param->name, p->name) == 0) {
                return index;
            }

            p++;
            index++;
        }

        gp++;
    }

    return -1;
}

int param_get_index_by_name(char* param_name)
{
    uint32_t index = 0;
    param_t* p;
    param_group_t* gp = (param_group_t*)&param_list;
    for(int j = 0 ; j < PARAM_GROUP_COUNT ; j++) {
        p = gp->content;
        for(int i= 0 ; i < gp->param_num ; i++) {
            if(strcmp(param_name, p->name) == 0)
                return index;
            p++;
            index++;
        }
        gp++;
    }
    
    return index;
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   val  
  * @param[out]  
  * @retval      
  * @note        用于接收地面站设置参数的命令，并保存
  */
int param_set_val(param_t* param, void* val)
{
    float _value = *((float *)val);
    float v = 0;

    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;
    
    switch (param->type) {
    case PARAM_TYPE_INT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -128, 127);
        param->val.i8 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i8, sizeof(int8_t));
        }
        break;

    case PARAM_TYPE_UINT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 255);
        param->val.u8 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u8, sizeof(uint8_t));
        }
        break;

    case PARAM_TYPE_INT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -32768, 32767);
        param->val.i16 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i16, sizeof(int16_t));
        }
        break;

    case PARAM_TYPE_UINT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 65535);
        param->val.u16 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u16, sizeof(uint16_t));
        }
        break;

    case PARAM_TYPE_INT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, INT32_MIN, INT32_MAX);
        param->val.i32 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i32, sizeof(int32_t));
        }
        break;

    case PARAM_TYPE_UINT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, UINT32_MAX);
        param->val.u32 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u32, sizeof(uint32_t));
        }
        break;

    case PARAM_TYPE_FLOAT:
        param->val.f = _value;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.f, sizeof(float));
        }
        break;

    case PARAM_TYPE_DOUBLE:
        param->val.lf = _value;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.lf, sizeof(double));
        }
        break;

    default:
        return -1;
    }
    
    param_save();

    return 0;
}

int param_set_val_by_full_name(char* group_name, char* param_name, char* val)
{
    param_t* p = param_get_by_full_name(group_name, param_name);
    if(p != NULL){
        if (p->type == PARAM_TYPE_INT8) {
            p->val.i8 = atoi(val);
        }

        if (p->type == PARAM_TYPE_UINT8) {
            p->val.u8 = atoi(val);
        }

        if (p->type == PARAM_TYPE_INT16) {
            p->val.i16 = atoi(val);
        }

        if (p->type == PARAM_TYPE_UINT16) {
            p->val.u16 = atoi(val);
        }

        if (p->type == PARAM_TYPE_INT32) {
            p->val.i32 = atoi(val);
        }

        if (p->type == PARAM_TYPE_UINT32) {
            p->val.u32 = atoi(val);
        }

        if (p->type == PARAM_TYPE_FLOAT) {
            p->val.f = atof(val);
        }

        if (p->type == PARAM_TYPE_DOUBLE) {
            p->val.lf = atof(val);
        }
    }else{
        rt_kprintf("can not find %s in group %s\n", param_name, group_name);
        return 1;
    }

    switch (p->type) {
    case PARAM_TYPE_INT8:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.i8, sizeof(int8_t));
        }
        break;

    case PARAM_TYPE_UINT8:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.u8, sizeof(uint8_t));
        }
        break;

    case PARAM_TYPE_INT16:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.i16, sizeof(int16_t));
        }
        break;

    case PARAM_TYPE_UINT16:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.u16, sizeof(uint16_t));
        }
        break;

    case PARAM_TYPE_INT32:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.i32, sizeof(int32_t));
        }
        break;

    case PARAM_TYPE_UINT32:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.u32, sizeof(uint32_t));
        }
        break;

    case PARAM_TYPE_FLOAT:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.f, sizeof(float));
        }
        break;

    case PARAM_TYPE_DOUBLE:
        if (p->user_param) {
            rt_memcpy(p->user_param, &p->val.lf, sizeof(double));
        }
        break;

    default:
        break;
    }

    return 0;
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   variable  
  * @param[out]  
  * @retval      
  * @note        将功能模块中的参数和参数模块中的对应参数链接起来，
  *              并获取参数模块中对应参数的值。
  */
void param_link_variable(param_t* param, void* variable)
{
    if (!param) {
        brd_config_error("[param]: <link variable> param is NULL");
    }

    param->user_param = variable;

    switch (param->type) {
    case PARAM_TYPE_INT8:
        rt_memcpy(variable, &param->val.i8, sizeof(int8_t));
        break;

    case PARAM_TYPE_UINT8:
        rt_memcpy(variable, &param->val.u8, sizeof(uint8_t));
        break;

    case PARAM_TYPE_INT16:
        rt_memcpy(variable, &param->val.i16, sizeof(int16_t));
        break;

    case PARAM_TYPE_UINT16:
        rt_memcpy(variable, &param->val.u16, sizeof(uint16_t));
        break;

    case PARAM_TYPE_INT32:
        rt_memcpy(variable, &param->val.i32, sizeof(int32_t));
        break;

    case PARAM_TYPE_UINT32:
        rt_memcpy(variable, &param->val.u32, sizeof(uint32_t));
        break;

    case PARAM_TYPE_FLOAT:
        rt_memcpy(variable, &param->val.f, sizeof(float));
        break;

    case PARAM_TYPE_DOUBLE:
        rt_memcpy(variable, &param->val.lf, sizeof(double));
        break;

    default:
        return;
    }
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   _value  
  * @param[out]  
  * @retval      
  * @note        设置被连接的参数的值
  */
void param_set_obj(param_t* param, double _value)
{
    double v = 0;

    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;

    if (!param->user_param) {
        brd_config_error("[param]: <%s> no link module parameters", param->name);
    }

    switch (param->type) {
    case PARAM_TYPE_INT8: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -128, 127);

        int8_t val = (int8_t)v;
        rt_memcpy(param->user_param, &val, sizeof(int8_t));
        } break;

    case PARAM_TYPE_UINT8: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 255);

        uint8_t val = (uint8_t)v;
        rt_memcpy(param->user_param, &val, sizeof(uint8_t));
        } break;

    case PARAM_TYPE_INT16: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -32768, 32767);

        int16_t val = (int16_t)v;
        rt_memcpy(param->user_param, &val, sizeof(int16_t));
        } break;

    case PARAM_TYPE_UINT16: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 65535);

        uint16_t val = (uint16_t)v;
        rt_memcpy(param->user_param, &val, sizeof(uint16_t));
        } break;

    case PARAM_TYPE_INT32: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, INT32_MIN, INT32_MAX);

        int32_t val = (int32_t)v;
        rt_memcpy(param->user_param, &val, sizeof(int32_t));
        } break;

    case PARAM_TYPE_UINT32: {
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, 0, UINT32_MAX);

        uint32_t val = (uint32_t)v;
        rt_memcpy(param->user_param, &val, sizeof(uint32_t));
        } break;

    case PARAM_TYPE_FLOAT: {
        float val = _value;
        rt_memcpy(param->user_param, &val, sizeof(float));
        } break;

    case PARAM_TYPE_DOUBLE: {
        double val = _value;
        rt_memcpy(param->user_param, &val, sizeof(double));
        } break;

    default:
        return;
    }
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   _value  
  * @param[out]  
  * @retval      
  * @note        设置被连接的参数的值,并将此参数发送到地面站
  */
void param_set_obj_and_notify(param_t* param, double _value)
{
    param_set_obj(param, _value);
    mavlink_param_send(param);
}

/**
  * @brief       
  * @param[in]   param  
  * @param[out]  
  * @retval      
  * @note        
  */
void param_save_obj(param_t* param)
{
    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;

    if (!param->user_param) {
        brd_config_error("[param]: <%s> no link module parameters", param->name);
    }

    switch (param->type) {
    case PARAM_TYPE_INT8:
        rt_memcpy(&param->val.i8, param->user_param, sizeof(int8_t));
        break;

    case PARAM_TYPE_UINT8:
        rt_memcpy(&param->val.u8, param->user_param, sizeof(uint8_t));
        break;

    case PARAM_TYPE_INT16:
        rt_memcpy(&param->val.i16, param->user_param, sizeof(int16_t));
        break;

    case PARAM_TYPE_UINT16:
        rt_memcpy(&param->val.u16, param->user_param, sizeof(uint16_t));
        break;

    case PARAM_TYPE_INT32:
        rt_memcpy(&param->val.i32, param->user_param, sizeof(int32_t));
        break;

    case PARAM_TYPE_UINT32:
        rt_memcpy(&param->val.u32, param->user_param, sizeof(uint32_t));
        break;

    case PARAM_TYPE_FLOAT:
        rt_memcpy(&param->val.f, param->user_param, sizeof(float));
        break;

    case PARAM_TYPE_DOUBLE:
        rt_memcpy(&param->val.lf, param->user_param, sizeof(double));
        break;

    default:
        return;
    }

    if (brd_is_vehicle_init_finish()) {
        param_save();
    } else {
        param_save_to_file();
    }
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   _value  
  * @param[out]  
  * @retval      
  * @note        设置被连接的参数和参数模块中对应变量的值，并保存到sd卡中
  */
void param_set_and_save(param_t* param, double _value)
{
    double v = 0;

    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;

    if (!param->user_param) {
        brd_config_error("[param]: <%s> no link module parameters", param->name);
    }

    switch (param->type) {
    case PARAM_TYPE_INT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -128, 127);
        param->val.i8 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i8, sizeof(int8_t));
        }
        break;

    case PARAM_TYPE_UINT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 255);
        param->val.u8 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u8, sizeof(uint8_t));
        }
        break;

    case PARAM_TYPE_INT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -32768, 32767);
        param->val.i16 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i16, sizeof(int16_t));
        }
        break;

    case PARAM_TYPE_UINT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 65535);
        param->val.u16 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u16, sizeof(uint16_t));
        }
        break;

    case PARAM_TYPE_INT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, INT32_MIN, INT32_MAX);
        param->val.i32 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.i32, sizeof(int32_t));
        }
        break;

    case PARAM_TYPE_UINT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, 0, UINT32_MAX);
        param->val.u32 = v;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.u32, sizeof(uint32_t));
        }
        break;

    case PARAM_TYPE_FLOAT:
        param->val.f = (float)_value;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.f, sizeof(float));
        }
        break;

    case PARAM_TYPE_DOUBLE:
        param->val.lf = _value;

        if (param->user_param) {
            rt_memcpy(param->user_param, &param->val.lf, sizeof(double));
        }
        break;

    default:
        return;
    }
    
    if (brd_is_vehicle_init_finish()) {
        param_save();
    } else {
        param_save_to_file();
    }
}

/**
  * @brief       
  * @param[in]   param  
  * @param[in]   _value  
  * @param[out]  
  * @retval      
  * @note        如果值被改变，设置被连接的参数和参数模块中对应变量的值，并保存到sd卡中
  */
void param_set_and_save_ifchanged(param_t* param, double _value)
{
    double v = 0;
    bool should_save = false;

    // add a small amount before casting parameter values
    // from float to integer to avoid truncating to the
    // next lower integer value.
    float rounding_addition = 0.01f;

    if (!param->user_param) {
        brd_config_error("[param]: <%s> no link module parameters", param->name);
    }

    switch (param->type) {
    case PARAM_TYPE_INT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -128, 127);

        if (param->val.i8 != (int8_t)v) {
            int8_t val = (int8_t)v;
            rt_memcpy(param->user_param, &val, sizeof(int8_t));

            param->val.i8 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_UINT8:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 255);

        if (param->val.u8 != (uint8_t)v) {
            uint8_t val = (uint8_t)v;
            rt_memcpy(param->user_param, &val, sizeof(uint8_t));
            
            param->val.u8 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_INT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, -32768, 32767);

        if (param->val.i16 != (int16_t)v) {
            int16_t val = (int16_t)v;
            rt_memcpy(param->user_param, &val, sizeof(int16_t));
            
            param->val.i16 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_UINT16:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_float(v, 0, 65535);

        if (param->val.u16 != (uint16_t)v) {
            uint16_t val = (uint16_t)v;
            rt_memcpy(param->user_param, &val, sizeof(uint16_t));
            
            param->val.u16 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_INT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, INT32_MIN, INT32_MAX);

        if (param->val.i32 != (int32_t)v) {
            int32_t val = (int32_t)v;
            rt_memcpy(param->user_param, &val, sizeof(int32_t));
            
            param->val.i32 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_UINT32:
        if (_value < 0) rounding_addition = -rounding_addition;
        v = _value+rounding_addition;
        v = math_constrain_double(v, 0, UINT32_MAX);

        if (param->val.u32 != (uint32_t)v) {
            uint32_t val = (uint32_t)v;
            rt_memcpy(param->user_param, &val, sizeof(uint32_t));
            
            param->val.u32 = v;
            should_save = true;
        }
        break;

    case PARAM_TYPE_FLOAT: {
        float val = v;
        if (!math_flt_equal(param->val.f,val)) {
            rt_memcpy(param->user_param, &val, sizeof(float));
            
            param->val.f = val;
            should_save = true;
        }
        } break;

    case PARAM_TYPE_DOUBLE:
        if (!math_flt_equal(param->val.lf,_value)) {
            rt_memcpy(param->user_param, &_value, sizeof(double));
            param->val.lf = _value;
            should_save = true;
        }
        break;

    default:
        return;
    }

    if (should_save) {
        if (brd_is_vehicle_init_finish()) {
            param_save();
        } else {
            param_save_to_file();
        }
    }
}

void param_save_to_file(void)
{
    int fd, size,slen;
    if(fs_init_complete()){
        fd = open(PARAM_FILE_NAME, O_WRONLY | O_CREAT);
        if(fd >= 0){

            /* add title */
            fs_fprintf(fd, "<?xml version=\"1.0\"?>\n");
            /* add param_list element */
            fs_fprintf(fd, "<param_list>\n");

            param_t* p;
            param_group_t* gp = (param_group_t*)&param_list;

            uint8_t group_num = sizeof(param_list)/sizeof(param_group_t);
            uint8_t group_num_half = group_num/2;

            for(int j = 0 ; j < group_num ; j++){
                /* add group element */
                fs_fprintf(fd, "\x20\x20<group name=\"%s\">\n", gp->name);
                
                p = gp->content;
                for(int i= 0 ; i < gp->param_num ; i++){
                    /* add param element */
                    fs_fprintf(fd, "\x20\x20\x20\x20<param name=\"%s\">\n", p->name);
                    
                    /* add value element */
                    if (p->type == PARAM_TYPE_INT8) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.i8);
                    }

                    if (p->type == PARAM_TYPE_UINT8) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.u8);
                    }

                    if (p->type == PARAM_TYPE_INT16) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.i16);
                    }

                    if (p->type == PARAM_TYPE_UINT16) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.u16);
                    }

                    if (p->type == PARAM_TYPE_INT32) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.i32);
                    }

                    if (p->type == PARAM_TYPE_UINT32) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%d</value>\n", p->val.u32);
                    }

                    if (p->type == PARAM_TYPE_FLOAT) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%f</value>\n", p->val.f);
                    }

                    if (p->type == PARAM_TYPE_DOUBLE) {
                        fs_fprintf(fd, "\x20\x20\x20\x20\x20\x20<value>%lf</value>\n", p->val.lf);
                    }
                    
                    p++;
                    write(fd, "\x20\x20\x20\x20</param>\n", strlen("\x20\x20\x20\x20</param>\n"));
                }
                
                gp++;
                fs_fprintf(fd, "\x20\x20</group>\n");

                if (j == group_num_half) {
                    fsync(fd);
                }
            }
            
            fs_fprintf(fd, "</param_list>\n");
            fsync(fd);
            
            close(fd);
        }else {
        }
    }
}

void param_load(void)
{
    int fd;
    char c;
    int size = 1;
    yxml_ret_t yxml_r;
    struct stat sta_f;
    
    fd = open(PARAM_FILE_NAME, O_RDONLY);
    
    PARAM_PARSE_STATE status = PARAM_PARSE_START;
    
    if(fd >=0){
        char *yxml_stack = (char*)rt_malloc(YXML_STACK_SIZE);
        
        int res_f = fstat(fd,&sta_f);
        if(res_f != RT_EOK) {
            rt_kprintf("param: can not open the file:%s err:%d\n", PARAM_FILE_NAME, res_f);
            return;
        }
        
        uint32_t file_size = sta_f.st_size;
        if(yxml_stack != NULL){
            yxml_t yxml_handle;
            yxml_init(&yxml_handle, yxml_stack, YXML_STACK_SIZE);
            while(file_size--){
                size = read(fd, &c, 1);
                
                if(size == 1){
                    yxml_r = yxml_parse(&yxml_handle, c);
                    _parse_xml(&yxml_handle, yxml_r, &status);
                }
                else{
                    rt_kprintf("xml file read err\n");
                    break;
                }
            }
            
            if(yxml_eof(&yxml_handle) < 0)
                rt_kprintf("xml parse err\n");
            else{
                rt_kprintf("parameter load success!\n");
            }
        }else{
            rt_kprintf("param malloc fail\n");
        }
        rt_free(yxml_stack);
    }else{
        rt_kprintf("can not find %s, use default parameters.\n", PARAM_FILE_NAME);
    }
    close(fd);
}

void param_init(void)
{
    DIR *dirp;
    int ret;

    /* 打开 / dir_test 目录 */
    dirp = opendir(PARAM_DIR);
    if (dirp == RT_NULL)
    {
        /* 创建目录 */
        ret = mkdir(PARAM_DIR, 0x777);
        if (ret < 0)
        {
            /* 创建目录失败 */
            rt_kprintf("dir error!\n");
        }
        else
        {
            /* 创建目录成功 */
            rt_kprintf("mkdir ok!\n");
        }
    }

    param_interface_init();
    
    param_load();
    param_save();
}

/*------------------------------------test------------------------------------*/


